// INCLUDES --------------------------------------------------------------------
#include "Vector4.h"

// FUNCTIONS -------------------------------------------------------------------

/* GENERAL:
   d    -> destination
   s    -> source
   size -> array size (0,1 for single elements)

   a,b  -> two arbitrary vertices
   k    -> floating point scalar
*/

static i32 i;       // loop variable
static fp32 factor; // multiplication factor

// clear to [0,0,0,1]
void V4Clear(Vector4 *v, ui32 size)
{
  GUARD_NULL("V4Clear *v", v);

  // clamp range to at least one element
  CLAMP_LOW(size, 1); 

  memset(v, 0, sizeof(Vector4)*size);       // clear vector to [0,0,0,0]
  for (i = 0; i < size; i++) v[i].w = 1.0f; // set w=1
};

// clone vector src->dest
void V4Copy(Vector4 *d, Vector4 *s, ui32 size)
{
  GUARD_NULL("V4Copy *d", d);
  GUARD_NULL("V4Copy *s", s);
  
  CLAMP_LOW(size, 1);                 // clamp range to at least one element
  memcpy(d, s, sizeof(Vector4)*size); // copy vector contents
};

/* the vector magnitude is defined by the pythagorean theorem for
   finding the length of a rectangular solid:

   |a| = sqrt(a² + b² + c²)

   which yields here to:

   |v| = sqrt(v.x² + v.y² + v.z²)

*/
fp32 V4Magnitude(Vector4 *v)
{
  GUARD_NULL("V4Magnitude *v", v, 0.0f);

  return sqrt(v->x * v->x +
              v->y * v->y +
              v->z * v->z);
};

/* a unit vector (denoted as â) is defined as the division of each of it's
   components by the vectors magnitude:

        a
   â = ---
       |a|

   so the result is basically:

        ax        ay        az
   âx = ---  ây = ---  âz = ---
        |a|       |a|       |a|

*/
void V4Normalize(Vector4 *v)
{
  fp32 magnitude = V4Magnitude(v);
  GUARD_ZERO("V4Normalize Magnitude", magnitude);

  factor = 1.0f / magnitude;
  V4MulTo(v, magnitude);
};

/* inverse vector from a to -a. this is done by negating every component.
   thus we retrieve:

   -a = [-(x) -(y) -(z)]
*/
void V4Invert(Vector4 *v) { V4MulTo(v, -1.0f); };

/* the scalar product (also refered to as the "dot product") is the
   resulting scalar of the sum of the products of the individual
   vector components of a and b.

   it's defined as:

   a * b = ax * bx + ay * by + az * bz

*/
fp32 V4ScalarP(Vector4 *a, Vector4 *b)
{
  GUARD_NULL("V4ScalarP *a", a, 0.0f);
  GUARD_NULL("V4ScalarP *b", b, 0.0f);

  return (a->x * b->x +
          a->y * b->y +
          a->z * b->z);
};

/* the vector product is defined by the expansion of the matrix

             i   j   k
   a x b = | ax  ay  az | = c
           | bx  by  bz |

   which expands to:

   a x b = [ (ay * bz - az * by)i
            -(ax * bz - az * bx)j
             (ax * by - ay * bx)k ]

   the resulting vector c is perpendicular both to a and c.

*/
void V4VectorP(Vector4 *d, Vector4 *a, Vector4 *b)
{
  GUARD_NULL("V4VectorP *d", d);
  GUARD_NULL("V4VectorP *a", a);
  GUARD_NULL("V4VectorP *b", b);

  d->x =   a->y * b->z - a->z * b->y;
  d->y = -(a->x * b->z - a->z * b->x);
  d->z =   a->x * b->y - a->y * b->x;
};

// [+, +=] ---------------------------------------------------------------------
/* vector addition:

   vector addition is done by adding up each individual component of the vector a
   with the corresponding component of the second vector b.

   thus we retrieve:

   a + b = [ax+bx ay+by az+bz]

*/
void V4Add(Vector4 *d, Vector4 *a, Vector4 *b)
{
  GUARD_NULL("V4Add *d", d);
  GUARD_NULL("V4Add *a", a);
  GUARD_NULL("V4Add *b", b);

  d->x = a->x + b->x;
  d->y = a->y + b->y;
  d->z = a->z + b->z;
};

void V4AddTo(Vector4 *d, Vector4 *a)
{
  GUARD_NULL("V4AddTo *d", d);
  GUARD_NULL("V4AddTo *a", a);

  d->x += a->x;
  d->y += a->y;
  d->z += a->z;
};

// [-, -=] ---------------------------------------------------------------------
/* vector subtraction:

   vector subtraction is done by subtracting each individual component
   of the vector a with the corresponding component of the second vector b.

   thus we retrieve:

   a - b = [ax-bx ay-by az-bz]

*/
void V4Sub(Vector4 *d, Vector4 *a, Vector4 *b)
{
  GUARD_NULL("V4Sub *d", d);
  GUARD_NULL("V4Sub *a", a);
  GUARD_NULL("V4Sub *b", b);

  d->x = a->x - b->x;
  d->y = a->y - b->y;
  d->z = a->z - b->z;
};

void V4SubTo(Vector4 *d, Vector4 *a)
{
  GUARD_NULL("V4SubTo *d", d);
  GUARD_NULL("V4SubTo *a", a);

  d->x -= a->x;
  d->y -= a->y;
  d->z -= a->z;
};

// [*, *=] ---------------------------------------------------------------------
/* scalar multiplication:

   scalar multiplication is done by multiplicating each individual component
   of the vector a with the given scalar k.

   thus we retrieve:

   ka = [k*ax k*ay k*az]

*/
void V4Mul(Vector4 *d, Vector4* a, fp32 k)
{
  GUARD_NULL("V4Mul *d", d);
  GUARD_NULL("V4Mul *a", a);

  d->x = a->x * k;
  d->y = a->y * k;
  d->z = a->z * k;
};

void V4MulTo(Vector4 *d, fp32 k)
{
  GUARD_NULL("V4MulTo *d", d);

  d->x *= k;
  d->y *= k;
  d->z *= k;
};

// [/, /=] ---------------------------------------------------------------------
/* scalar division:

   scalar division is done by dividing each individual component
   of the vector a with the given scalar k.

   thus we retrieve:

   a / k = [ax/k ay/k az/k]

*/
void V4Div(Vector4 *d, Vector4* a, fp32 k)
{
  GUARD_ZERO("V4Div k", k);

  factor = 1.0f / k;
  V4Mul(d, a, factor);
};

void V4DivTo(Vector4 *d, fp32 k)
{
  GUARD_ZERO("V4DivTo k", k);

  factor = 1.0f / k;
  V4MulTo(d, factor);
};

// [==, !=] --------------------------------------------------------------------
bool V4Equals(Vector4 *a, Vector4 *b)
{
  GUARD_NULL("V4Equals *a", a, false);
  GUARD_NULL("V4Equals *b", b, false);

  return (a->x == b->x &&
          a->y == b->y &&
          a->z == b->z);
};

bool V4InEquals(Vector4 *a, Vector4 *b) { return !V4Equals(a, b); };
